'''
Created on Aug 27, 2010

@author: alia
'''
import logging
log = logging.getLogger('steer')
log.debug("imported")
 
from cocos.euclid import *
from cocos import draw
import constants
# Adapted from Daniel Shiffman <http://www.shiffman.net>

class Steer(object):
    MAX_SPEED = 140.0
    MAX_STEER_FORCE = 10.0
    SLOW_DISTANCE = 2
    TARGET_INFLUENCE = 0.5
    MIN_INFLUENCE_DISTANCE =  50
    PATH_RADIUS = 10.0
    PATH_INTERVAL = 20.0

    def __init__(self):
        '''
        Constructor
        '''
        
        
        self.accumulator =  Vector2(0, 0)
        self.force_accumulator =  Vector2(0, 0)
        self.acceleration =  Vector2(0, 0)
        self.velocity =  Vector2(0, 0)
        self.pos =  Vector2(0, 0)
        
        if constants.DEBUG:
            self.debug = {
                          "predict": draw.Line(self.pos,self.pos+Vector2(0.1, 0.1), (0,255,0,255), 2),
                          }
            for line in self.debug.values():
                self.add(line)

        self.path = []
        self.target =  None
        self.arrive = True
        self.best_index = 0
        
    # To pull the path off list and remove the image
    def nasty_hack(self):   
        if self.mouse is not None:
            self.mouse.update_path()
            
    def steer(self, target, slowdown=True ):
        log.debug("steer: "+str((target, slowdown)))
        steer = Vector2(0, 0)
        desired = target-self.pos
        dist = desired.magnitude()

        if dist > 0:
            desired.normalize()

            if slowdown and (dist < self.SLOW_DISTANCE):
                log.debug("steer: slowing down "+str(dist))
                
                desired *= self.MAX_STEER_FORCE*(dist/self.SLOW_DISTANCE)
            else:
                desired *= self.MAX_STEER_FORCE
                
            steer = desired - self.velocity
            
            
            
            if steer.magnitude() >  self.MAX_STEER_FORCE:
                steer /= steer.magnitude() /   self.MAX_STEER_FORCE
        else:
            log.debug("steer: arrived - going to next point ")
            # goto the next point
            self.nasty_hack()
                
        log.debug("steer: "+str((steer, steer.magnitude())))    

        return steer
    
    def get_normal_point(self, point, a, b):
        ap = point - a
        ab = b - a
        ab.normalize()
        ab *= ap.dot(ab)

        return a+ab;
        
    def follow_path(self):
        length = len(self.path) 
        
        predict = self.pos + (self.velocity.normalized()*25) # 25 frames ahead
        log.debug("follow_path length: "+str(length )+" predict: "+str(predict))
        
        target = None
        dir = None
        best = 1000000
         
        self.best_index = 0 
        for i in range(length):
            a = self.path[i].start
            b = self.path[i].end
            if a == b:
                continue
            
            normal = self.get_normal_point(predict, a, b)
            da = (normal - a).magnitude()
            db = (normal - b).magnitude()
            
            line = b-a

            if (i+1< length) and (da + db > line.magnitude()+1):
                normal = b
          
            d = (predict-normal).magnitude()
            if (d < best):
                best = d
                target = normal
                dir = line
                dir.normalize()
                dir *= 10
                self.best_index = i
        
        log.debug("follow_path best index: "+str((self.best_index, self.path[self.best_index].start,self.path[self.best_index].end)))
                
        if (target is not None and dir is not None) and (best > self.PATH_RADIUS or self.velocity.magnitude() < 0.1):
            target += dir
            log.debug("follow_path target: "+str(target))
            return self.steer(target, False)
        else:
            log.debug("MEH: follow_path target: "+str(target)+" dir: "+str(dir)+str((best , self.PATH_RADIUS , self.velocity.magnitude())))
            return Vector2(0,0)
         
    def accumulate_steer_forces(self):
        log.debug("accumulate_steer_forces")

        if self.path is not None and len(self.path) > 0:
            log.debug("accumulate_steer_forces following path")
            self.accumulator += self.follow_path()*self.TARGET_INFLUENCE
                    
        if self.target is not None:
            log.debug("accumulate_steer_forces steering towards target "+str(self.target))
            steer = self.steer(self.target, self.arrive)
            self.accumulator += steer*self.TARGET_INFLUENCE     
        
        # Apply steering forces
        self.acceleration += self.accumulator
        
        log.debug("accumulated steer forces: "+str(self.accumulator))
        
        #if self.acceleration.magnitude_squared() >  self.MAX_STEER_FORCE*self.MAX_STEER_FORCE:
        #    self.acceleration.normalize()
            
        log.debug("acceleration: "+str(self.acceleration)+" force acc: "+str(self.force_accumulator))
           
        # Apply other forces (like gravity) that dont have bounds
        self.acceleration += self.force_accumulator
            
    def position_update(self, dt):
        log.debug("position_update "+str(dt))
        
        self.accumulate_steer_forces()
        
        self.velocity = self.acceleration#*dt - we are doing in frames
        
        if  self.velocity.magnitude_squared() >  self.MAX_SPEED*self.MAX_SPEED:
            self.velocity.normalize()
            self.velocity *= self.MAX_SPEED
            
        log.debug("velocity: "+str(self.velocity))
        
        self.pos += self.velocity
        #self.position = self.position[0] +self.velocity.x , self.position[1] +self.velocity.y 
         
        self.accumulator = Vector2(0, 0)
        self.acceleration = Vector2(0, 0)
      